#ifndef __ESTIMATOR_HPP
#define __ESTIMATOR_HPP

#include "common_include.hpp"
#include "Config.hpp"

namespace visionhw
{

    class Estimator
    {
    public:
        Estimator();
        ~Estimator() {}
        void setInputSource(const CloudType::Ptr cloud_source,
                            const pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_source);
        void setInputTarget(const CloudType::Ptr cloud_target,
                            const pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_target);
        void estimate(CloudType &cloud_output);
        inline Eigen::Matrix4f getFinalIcpTrans(){return _icp_trans;}
        inline Eigen::Matrix4f getFinalSacTrans(){return _sac_trans;}
        inline auto getSacTimeCost() {return _sac_time_used_ms;}
        inline auto getIcpTimeCost() {return _icp_time_used_ms;}
        inline void setSaveIntermediatePly(bool flag) { _saveIntermediatePly = flag;}        

    private:
        void init();
        CloudType::Ptr _cloud_src;
        CloudType::Ptr _cloud_tgt;
        pcl::PointCloud<pcl::FPFHSignature33>::Ptr _fpfh_src;
        pcl::PointCloud<pcl::FPFHSignature33>::Ptr _fpfh_tgt;
        // sac-ia
        pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> _sac_ia;
        CloudType::Ptr _sac_result;
        Eigen::Matrix4f _sac_trans;

        //icp
        CloudType::Ptr _icp_result;
        pcl::GeneralizedIterativeClosestPoint<PointType, PointType> _icp;
        Eigen::Matrix4f _icp_trans;
    
    private:
        bool _saveIntermediatePly = false;
        double _sac_time_used_ms;
        double _icp_time_used_ms;
        

    };
}

#endif